#include <manual/input.h>

/**
 * 	Joystick input callback.
 * 
 * 	This handles input received from a connected joystick device
 * 	and transmits it as Move commands to the motor_controller.
 **/
void Input::joystick_callback (const sensor_msgs::Joy::ConstPtr & joy)
{
	ROS_INFO("Joystick callback called.");
	
	double	x	= joy -> axes[0];
	double	y	= joy -> axes[1];
	double 	angle 	= 0.0f;
	
	if (x == 0 && y == 0)
	  return;
	
	if (y == 0)
	  angle 	= (x > 0) ? 0.5 * M_PI : -0.5 * M_PI;
	else
	  angle 	= atan(x / abs(y));
	if (y < 0)
	  angle  	= M_PI - angle;
	  
	ROS_INFO("Angle: %f (%f, %f)\n", angle, x, y);
	
	publish(sqrt(pow(x, 2) + pow(y, 2)), angle);
}